/*
 * @Author: newsun
 * @Date: 2022-01-11 19:34:49
 * @LastEditTime: 2022-02-15 16:46:43
 * @LastEditors: newsun-HP-Pavilion-Gaming-Laptop-15-dk0xxx
 * @Description: 反陀螺模块
 * @FilePath: /Vision_bak/src/armor/include/aim_spin/AntiSpin.hpp
 */

#ifndef _ANTI_SPIN_HPP
#define _ANTI_SPIN_HPP
#include <opencv2/opencv.hpp>
#include <opencv2/highgui.hpp>
#include <algorithm>
#include "readParam.hpp"
#include "../aim_deps/AimDeps.hpp"
#include "../aim_pre/Predict.hpp"
#include "serial_com/comm.h"
#include <Eigen/Geometry>
#include <Eigen/Cholesky>
#include <Eigen/Core>

#define HISTORY_SIZE 60
class AntiSpin{
private:
    double init_time;

    std::deque<float> xs_history;//历史的x值，头进尾出
    std::deque<float> ts_history;//与x值对应的时间戳
    bool is_spin;//在小陀螺为1否则为0
    float mid_avg;//陀螺中心x
public:
    AntiSpin();
    ~AntiSpin(){}
    void anti(const cv::Point3f& t_cam, const serial_com::comm& msg,Eigen::Vector3d& cam_p);//暂时先不加入用数字去判断
    bool spining(){     //如果在旋转则返回1
        return is_spin;
    }

};







#endif